import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from sympy import *



pi = np.pi
cos = np.cos
sin = np.sin
arctan = np.arctan
arccos = np.arccos
g = 9.81
nx = 4
ny = 0
b=0.2
i = 0
j = 0
h = 0
dv = 0.  #速度差
dh = 0.  #高度差
dis = 0.  #与目标的距离
alpha = 0
beta = 0




second = 30                        #秒
dt = 0.01                     #采样时间
n = int(second / dt)
t = np.linspace(0, second, n)      #生成一个等间隔时间数组
position = np.zeros((n,8))  #生成一个(1000，6)的状态数组，七个状态分别为三个方向的坐标,速度，航迹倾斜角，航迹偏转角


velocity, thet, psi = 100., 0., 0.  #分别设初始速度为250m/s , 航迹倾斜角为0, 航迹偏转角为0
x,y,z = 0., 0., 0.  #初始位置,惯性坐标系

dthet = 0.
dpsi = 0.    #法向角速度的两个分量
ddthet = 0.
ddpsi = 0.   #两个角加速度

d_thet = 2.   #两个角度差
d_psi = 2.

position[0,0] = x; position[0,1] = y; position[0,2] = z;
position[0,3] = velocity; position[0,4] = thet; position[0,5] = psi;
position[0,6] = dthet; position[0,7] = dpsi;


target = np.zeros((7,4))
target[0, :] = [300, 200, 100, 300]
target[1, :] = [400, 500, 600, 150]
target[2, :] = [600, 800, 1000, 200]
target[3, :] = [1000, 1050, 700, 260]
target[4, :] = [900, 750, 1200, 280]
target[5, :] = [600, 550, 1000, 250]
target[6, :] = [300, 600, 8000, 400]


i = 1


for k in range(1, n):                            #开始计时
    d_x = target[i - 1, 0] - position[k - 1, 0]      # 位置向量x的差
    d_y = target[i - 1, 1] - position[k - 1, 1]      # 位置向量y的差
    dh = target[i - 1, 2] - position[k - 1, 2]      # 位置向量z的差
    dis = pow(d_x*d_x + d_y*d_y + dh * dh, 0.5)       # 计算与目标的绝对距离
    dv = target[i - 1, 3] - position[k - 1, 3]
    velocity = position[k - 1, 3]

    if abs(d_x) <5 and abs(d_y)<5 and abs(dh)<5:
        i = i + 1
        d_psi = 2
        d_thet = 2
    if i == 8:
        cnt = k
        break
    print(i)
    d_x = target[i - 1, 0] - position[k - 1, 0]  # 位置向量x的差
    d_y = target[i - 1, 1] - position[k - 1, 1]  # 位置向量y的差
    dh = target[i - 1, 2] - position[k - 1, 2]  # 位置向量z的差
    dxy = pow(d_x * d_x + d_y * d_y, 0.5)
    dis = pow(d_x * d_x + d_y * d_y + dh * dh, 0.5)  # 计算与目标的绝对距离  = dr
    print(dis)
    print(k)

    if abs(d_thet) > 0.00001 or abs(d_psi) > 0.00001:
        if d_x > 0 and d_y >= 0:
            beta = arctan(d_y/d_x)
        elif d_x < 0 and d_y >= 0:
            beta = pi + arctan(d_y / d_x)
        elif d_x == 0 and d_y >= 0:
            beta = pi/2
        elif d_x > 0 and d_y < 0:
            beta = arctan(d_y / d_x)
        elif d_x < 0 and d_y < 0:
            beta = pi + arctan(d_y / d_x)
        elif d_x == 0 and d_y < 0:
            beta = -pi/2

        if dh > 1 or dh < -1:
            alpha = arctan(dh / dxy)
        else:
            alpha = 0

        b = 0.03
        d_thet = alpha - position[k - 1, 4]  # 位置向量与速度向量的两个角度差
        d_psi = beta - position[k - 1, 5]
        print(d_psi)
        w = sqrt((d_thet * b - dthet)*(d_thet * b - dthet)+(d_psi * b - dpsi)*(d_psi * b - dpsi)) / dt
        ny = w / g
        while abs(ny) > 4:
            w = sqrt((d_thet * b - dthet) * (d_thet * b - dthet) + (d_psi * b - dpsi) * (d_psi * b - dpsi)) / dt
            ny = w / g
            b = b / 2


        psi = psi + d_psi * b
        thet = thet + d_thet * b
        dpsi = d_psi * b
        dthet = d_thet * b

    if abs(d_thet) < 0.00001 and abs(d_psi) < 0.00001:
        psi = psi
        thet = thet
    print(d_thet)
    print(d_psi)


    if dv > 0.1:
        velocity1 = velocity
        velocity = velocity + (nx - sin(thet)) * g * dt
        a = (velocity - velocity1) / dt
    elif dv < -0.1:
        velocity1 = velocity
        velocity = velocity - (nx - sin(thet)) * g * dt
        a = (velocity - velocity1) / dt
    elif abs(dv) < 0.1:
        velocity = velocity + dv


    vx = velocity * np.cos(thet) * np.cos(psi)
    vy = velocity * np.cos(thet) * np.sin(psi)
    vz = velocity * np.sin(thet)

    dx = vx * dt
    dy = vy * dt
    dz = vz * dt

    x = x + dx
    position[k, 0] = x
    y = y + dy
    position[k, 1] = y
    z = z + dz
    position[k, 2] = z
    print(x,'x')
    print(y,'y')
    print(z,'z')
    position[k, 3] = velocity
    position[k, 4] = thet
    position[k, 5] = psi
    position[k, 6] = dthet
    position[k, 7] = dpsi

fig = plt.figure()
ax = Axes3D(fig)
plt.title('trajectory')
for q in range(0, 5):
    ax.scatter(target[q, 0], target[q, 1], target[q, 2], c='r', marker='o')
    ax.text(target[q, 0], target[q, 1], target[q, 2], q)
for q in range(0,k):
    ax.plot(position[0:q, 0], position[0:q, 1], position[0:q, 2],c='b')
plt.xlabel("X")
plt.ylabel("Y")



#     if (abs>1):                                      # 当前位置与目标位置相差大于1，则进行更新操作
#         alpha = arctan(d_y/(pow(d_x*d_x+d_z*d_z,0.5)))         #由位置向量算出来的两个角
#         beta = arctan(d_z/d_x)
#         d_thet = alpha - position[k-1,4]                       #位置向量与速度向量的两个角度差
#         d_psi = beta - position[k-1,5]
#         dthet = 0.5 * d_thet                                     #由算出的差值给角速度进行赋值
#         dpsi = 0.5 * d_psi
#         ddthet = (dthet - position[k - 1, 6]) / dt
#         ddpsi = (dpsi - position[k - 1, 7]) / dt
#         while(pow(ddthet * ddthet + ddpsi * ddpsi,0.5) > 4*g):
#             dthet = 0.5 * dthet
#             dpsi = 0.5 * dpsi
#             print(dthet, "角速度1")
#             print(dpsi, "角速度2")
#             ddthet = (dthet - position[k - 1, 6]) / dt
#             ddpsi = (dpsi - position[k - 1, 7]) / dt
#             print(ddthet, "法向过载1")
#             print(ddpsi, "法向过载2")
#             print(pow(ddthet * ddthet + ddpsi * ddpsi,0.5),"法向过载")
#         position[k, 6] = dthet
#         position[k, 7] = dpsi
#         d_v = target[i - 1, 3] - position[k - 1, 3]
#
#
#         if (d_v < 0.01):                                  #如果速度不等的话更新速度
#             velocity = position[k - 1, 3] + g * (4 - sin(position[k - 1, 4])) * dt
#         elif(d_v > 0.01):
#             velocity = position[k - 1, 3] + g * (4 - sin(position[k - 1, 4])) * dt
#         else:
#             velocity = position[k - 1, 3]                                        #速度相等速度不变
#
#
#         if (np.abs(d_thet) > 0.0001 and np.abs(d_psi) > 0.0001):                                 #角度未达到目标的话更新角度
#             thet = position[k - 1, 4] + dthet * dt
#             psi = position[k - 1, 5] + dpsi * dt
#         elif(np.abs(d_thet) <= 0.0001 and np.abs(d_psi) > 0.0001):
#             thet = position[k - 1, 4]
#             psi = position[k - 1, 5] + dpsi * dt
#         elif(np.abs(d_thet) <= 0.0001 and np.abs(d_psi) <= 0.0001):
#             thet = position[k - 1, 4] + dthet * dt
#             psi = position[k - 1, 5]
#
#
#
#         x = position[k - 1, 0] + velocity * cos(thet) * cos(psi) * dt                            #更新位置
#         y = position[k - 1, 1] + velocity * sin(thet) * dt
#         z = position[k - 1, 2] + velocity * cos(thet) * sin(psi) * dt
#
#
#         position[k, 0] = x
#         position[k, 1] = y
#         position[k, 2] = z
#
#         position[k, 3] = velocity
#         position[k, 4] = thet
#         position[k, 5] = psi
#
#         print(position[k, 3], "速度")
#         print(position[k, 0], "x")
#         print(position[k, 1], "y")
#         print(position[k, 2], "z")
#
#     else:
#         i=i+1
#         continue
#
#
#
# fig = plt.figure()
# ax = Axes3D(fig)
# plt.title('trajectory')
# ax.scatter3D(position[:, 0], position[:, 1], position[:, 2],'gray')
# plt.xlabel("x")
# plt.ylabel("y")

plt.figure(3)
plt.title(r'velocity')
plt.plot(position[:,3])
plt.figure(4)
plt.title(r'hangjijiao')
plt.plot(position[:,4])
plt.figure(5)
plt.title(r'hangxiangjiao')
plt.plot(position[:,5])
plt.show()


